
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       ahrs_gcs.c
  * @author     baiyang
  * @date       2021-12-15
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "ahrs.h"

#include <stdint.h>
#include <stdbool.h>

#include <rthw.h>
#include <rtthread.h>
#include <rtdevice.h>

#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>
#include <common/time/gp_time.h>
#include <mavproxy/mavproxy.h>
#include <mavproxy/mavproxy_monitor.h>
#include <sensor_compass/sensor_compass.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/
typedef struct {
    uint8_t sysid;          ///< ID of message sender system/aircraft
    uint8_t compid;         ///< ID of the message sender component

    struct rt_ringbuffer msg_rb;       // id(4 byte) + sysid(1 byte) + compid(1 byte) + msg
    uint8_t msg_rb_pool[1024];

    mavlink_message_t mavlink_msg;

    struct rt_mutex _mutex;
} ahrs_gcs_msg;
/*---------------------------------prototype----------------------------------*/
static void _proc_command(mavlink_command_long_t* command, mavlink_message_t* msg);
static void _send_command_ack(uint16_t command, uint8_t result, mavlink_message_t* msg);

static void _gcs_cb(void *parameter);
static void _push_msg(uint32_t msg_id, uint8_t sysid, uint8_t compid, uint8_t *msg_buf, uint32_t msg_len);
static uint32_t _get_msg_id();
static void _get_msg_date(uint8_t *msg_buf, uint32_t msg_len);
static bool _should_handle_command_long(uint16_t command);
/*----------------------------------variable----------------------------------*/
static ahrs_gcs_msg gcs_msg;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void ahrs_gcs_init()
{
    rt_ringbuffer_init(&gcs_msg.msg_rb, gcs_msg.msg_rb_pool, sizeof(gcs_msg.msg_rb_pool));

    rt_mutex_init(&gcs_msg._mutex, "ahrs_gcs", RT_IPC_FLAG_FIFO);

    itc_subscribe(ITC_ID(mavlink_msg), _gcs_cb);
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void ahrs_gcs_update()
{
    uint32_t msg_id;
    uint16_t len = rt_ringbuffer_space_len(&gcs_msg.msg_rb);
    if (rt_ringbuffer_data_len(&gcs_msg.msg_rb) < sizeof(uint32_t)) {
        // 没有要处理的数据，直接返回
        return;
    }

    msg_id = _get_msg_id();

    switch (msg_id) {
        case MAVLINK_MSG_ID_COMMAND_LONG: {
            mavlink_command_long_t command;
            _get_msg_date((uint8_t *)&command, sizeof(mavlink_command_long_t));

            mavlink_msg_command_long_encode(gcs_msg.sysid, gcs_msg.compid, &gcs_msg.mavlink_msg, &command);
            _proc_command(&command, &gcs_msg.mavlink_msg);
        } break;

        default: {
            // console_printf("unknown mavlink msg:%d\n", msg->msgid);
        } break;
    }
}

static void _proc_command(mavlink_command_long_t* command, mavlink_message_t* msg)
{
    MAV_RESULT result = MAV_RESULT_FAILED;

    switch (command->command) {
    case MAV_CMD_DO_START_MAG_CAL:
    case MAV_CMD_DO_ACCEPT_MAG_CAL:
    case MAV_CMD_DO_CANCEL_MAG_CAL: {
        result = sensor_compass_handle_mag_cal_command(command);
        break;
    }

    default:
        break;
    }

    _send_command_ack(command->command, result, msg);
}

static void _send_command_ack(uint16_t command, uint8_t result, mavlink_message_t* msg)
{
    mavlink_command_ack_t command_ack = {command, result, 0, 0, 0, 0};
    mavlink_system_t sysid_this_mav = mavproxy_get_system();

    mavlink_msg_command_ack_encode(sysid_this_mav.sysid, sysid_this_mav.compid,
        msg, &command_ack);

    mavproxy_send_immediate_msg(msg, 1);
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static void _gcs_cb(void *parameter)
{
    uitc_mavlink_msg *mavlink_msg = (uitc_mavlink_msg *)parameter;
    uint32_t msg_id;
    uint8_t *msg_buf;
    uint32_t msg_len;

    switch (mavlink_msg->msg_t->msgid) {
        case MAVLINK_MSG_ID_COMMAND_LONG: {
            mavlink_command_long_t command;
            mavlink_msg_command_long_decode(mavlink_msg->msg_t, &command);

            if (_should_handle_command_long(command.command)) {
                _push_msg(MAVLINK_MSG_ID_COMMAND_LONG, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&command, sizeof(mavlink_command_long_t));
            }
        } break;
        
        default: {
            // console_printf("unknown mavlink msg:%d\n", msg->msgid);
        } break;
    }
}

/**
  * @brief       
  * @param[in]   msg_id  
  * @param[in]   msg_buf  
  * @param[in]   msg_len  
  * @param[out]  
  * @retval      
  * @note        
  */
static void _push_msg(uint32_t msg_id, uint8_t sysid, uint8_t compid, uint8_t *msg_buf, uint32_t msg_len)
{
    if (rt_ringbuffer_space_len(&gcs_msg.msg_rb) >= msg_len + sizeof(msg_id) + sizeof(sysid) + sizeof(compid)) {

        rt_mutex_take(&gcs_msg._mutex, RT_WAITING_FOREVER);

        rt_ringbuffer_put(&gcs_msg.msg_rb, (uint8_t *)&msg_id, sizeof(msg_id));
        rt_ringbuffer_putchar(&gcs_msg.msg_rb, sysid);
        rt_ringbuffer_putchar(&gcs_msg.msg_rb, compid);
        rt_ringbuffer_put(&gcs_msg.msg_rb, msg_buf, msg_len);

        rt_mutex_release(&gcs_msg._mutex);
    } else {
        mavproxy_send_statustext(MAV_SEVERITY_INFO, "fms_gcs miss msg");
    }
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static uint32_t _get_msg_id()
{
    uint32_t msg_id;

    rt_mutex_take(&gcs_msg._mutex, RT_WAITING_FOREVER);
    rt_ringbuffer_get(&gcs_msg.msg_rb, (uint8_t *)&msg_id, sizeof(msg_id));
    rt_mutex_release(&gcs_msg._mutex);

    return msg_id;
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static void _get_msg_date(uint8_t *msg_buf, uint32_t msg_len)
{
    rt_mutex_take(&gcs_msg._mutex, RT_WAITING_FOREVER);
    rt_ringbuffer_getchar(&gcs_msg.msg_rb, &gcs_msg.sysid);
    rt_ringbuffer_getchar(&gcs_msg.msg_rb, &gcs_msg.compid);
    rt_ringbuffer_get(&gcs_msg.msg_rb, msg_buf, msg_len);
    rt_mutex_release(&gcs_msg._mutex);
}

static bool _should_handle_command_long(uint16_t command)
{
    bool res = false;

    switch (command) {
        case MAV_CMD_DO_START_MAG_CAL:
        case MAV_CMD_DO_ACCEPT_MAG_CAL:
        case MAV_CMD_DO_CANCEL_MAG_CAL: {
            res = true;
        } break;

        default: {
            // console_printf("unknown mavlink msg:%d\n", msg->msgid);
        } break;
    }

    return res;
}

/*------------------------------------test------------------------------------*/


